#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <fcntl.h>
#include <termios.h>
#include <string.h>

#include "data_link.h"

char control_values[3] = {C_SET, C_UA, C_DISC};
/*
void stuff_buffer(char* buf, int buf_len, char* new_buf, int new_buf_len) {
	int i;
	int nbuf_offset = 0;

	for (i = 0; i < buf_len; i++) {
		if (buf[i] == FLAG || buf[i] == ESC) {
			new_buf[i + nbuf_offset] = ESC;
			new_buf[i + nbuf_offset + 1] = buf[i] ^ 0x20;
			nbuf_offset += 1;
		} else {
			new_buf[i + nbuf_offset] = buf[i];
		}
	}
}
*/
int stuff_buffer(char* argBuf, char* finalBuf, int size)
{
    int i = 0;
    int actualPos = 0;

    while(i < size)
    {
        if(argBuf[i] == FLAG)
        {
            finalBuf[actualPos] = ESC;
            finalBuf[actualPos+1] = 0x5e;
            i++;
            actualPos += 2;
        }
        else if(argBuf[i] == ESC)
        {
            finalBuf[actualPos] = ESC;
            finalBuf[actualPos+1] = 0x5d;
            i++;
            actualPos += 2;
        }
        else
        {
            finalBuf[actualPos] = argBuf[i];
            i++;
            actualPos++;
        }
    }

    return actualPos;
}

int stuff_len(char* buf, int len) {
	int i;
	int stuff_counter = 0;
	for (i = 0; i < len; i++) {
		if (buf[i] == FLAG || buf[i] == ESC) {
			stuff_counter += 1;
		}
	}
	return stuff_counter;
}

void destuff_buffer(char* buf, char buf_len, char* new_buf, int new_buf_len) {
	int i;
	int nbuf_offset = 0;
	for (i = 0; i < buf_len; i++) {
		if (buf[i] == ESC) {
			if (buf[i + 1] == 0x5e) //FLAG
				new_buf[i + nbuf_offset] = FLAG;
			else if (buf[i + 1] == 0x5d) //ESC
				new_buf[i + nbuf_offset] = ESC;
			nbuf_offset -= 1;
			i += 1;
		} else {
			new_buf[i + nbuf_offset] = buf[i];
		}
	}
}

int write_frame_header(int fd, char control) {
	int bytes_written = 0;
	char c[1] = { FLAG };
	bytes_written += write(fd, &c, 1);
	printf("%x\n", c[0]);

	c[0] = ADDRESS;
	bytes_written += write(fd, &c, 1);
	printf("%x\n", c[0]);

	c[0] = control;
	bytes_written += write(fd, &c, 1);
	printf("%x\n", c[0]);

	c[0] = ADDRESS ^ control;
	char new_buf[stuff_len(c, 1) + 1];
	stuff_buffer(c, new_buf, 1);
	bytes_written += write(fd, &new_buf, stuff_len(c,1)+1);
	printf("%x\n", c[0]);
	
	return bytes_written - stuff_len(c, 1);
}

void write_cframe(int fd, char control) {
	printf("\n---\nsending %x control frame\n", control);
	write_frame_header(fd, control);

	char c = FLAG;
	write(fd, &c, 1);
	printf("%x\n", c);

	printf("%x control frame sent\n---\n", control);
}

int write_iframe(int fd, char control, char* packet, int p_len) {
	int i;
	int bytes_written = 0;
	printf("\n---\nsending %x information frame\n", control);
	bytes_written += write_frame_header(fd, control);
	char new_buf[stuff_len(packet, p_len) + p_len];
	stuff_buffer(packet, new_buf, p_len);

	printf("packet: \n");
	for (i = 0; i < p_len; i++) {
		printf("%c", packet[i]);
	}
	printf("\nstuffed packet: \n");
	for (i = 0; i < stuff_len(packet, p_len) + p_len; i++)
		printf("%c", new_buf[i]);
	bytes_written += write(fd, &new_buf, stuff_len(packet,p_len)+p_len);

	char bcc2[1] = { packet[0] };
	for (i = 1; i < p_len; i++) {
		bcc2[0] = bcc2[0] ^ packet[i];
	}

	int bcc2_stuff_len = stuff_len(bcc2, 1) + 1;
	char bcc2_stuff[bcc2_stuff_len];
	stuff_buffer(bcc2, bcc2_stuff, 1);
	bytes_written += write(fd, &bcc2_stuff, stuff_len(bcc2,1)+1);
	for (i = 0; i < bcc2_stuff_len; i++)
		printf("%x\n", bcc2_stuff[i]);

	char c[1] = { FLAG };
	bytes_written += write(fd, &c, 1);
	printf("%x\n", c[0]);

	printf("%x information frame sent\n---\n", control);

	return bytes_written - stuff_len(packet, p_len);
}


int read_frame(int fd, char* new_buf)
{
	char buf = '1';
	char frame_buf[MAX_FRAME_SIZE];
	int frame_buf_i = 0;
	bzero(frame_buf,MAX_FRAME_SIZE);
	while(buf!=FLAG)
	{
		read(fd,&buf,1);
	}
	//recebi uma flag; comecar frame
	while(1)
	{
		read(fd,&buf,1);
		if(buf==FLAG) //final de frame
		{
			break;
		}
		else
		{
			frame_buf[frame_buf_i] = buf;
			frame_buf_i++;
		}
	}
	
	//apply destuff
	int new_len = MAX_FRAME_SIZE-frame_buf_i-stuff_len(frame_buf, frame_buf_i);
	destuff_buffer(frame_buf, frame_buf_i, new_buf, new_len);
	return new_len;
}

char check_frame_header(char* frame)
{
	if(frame[0] != ADDRESS)
		return 0xFF;
	if(frame[2] == BCC(frame[1]))
		return frame[1];
	return 0xFF;
}

int check_packet_integrity(char* pack, int len_p, char bcc2)
{
	char checksum = pack[0];
	int i;
	for(i=1; i<len_p; i++)
	{
		checksum = pack[i]^checksum;
	}
	if(checksum == bcc2)
		return 0; //bcc OK
	return -1; // bcc NOT OK
}

void extract_data_packet(char* pack, int len_p, char* data)
{
	int i;
	for(i=4;i<len_p;i++)
	{
		data[i-4] = pack[i];
	}
}

int extract_packet(char* frame, int len_f, char* packet, int len_p)
{
	int i;
	for(i=0;i<len_f-1;i++)
	{
		packet[i] = frame[i+3];
	}
	return check_packet_integrity(packet, len_p, frame[len_f]);
}


int readd(int fd, char* buf)
{
	int frame_l = read_frame(fd, buf);
	if (check_frame_header(buf)!= 0xFF) //header OK
	{
		int packet_l = frame_l-4;
		if(packet_l == -1) //SIGNIFICA QUE SE TRATA DE UMA TRAMA DE CONTROLO
		{
			int a = 0;
		}
		else
		{
			char data[packet_l-4]; //CASO PARTICULAR DE PACOTE DE DADOS 
			char packet[packet_l];
			if (extract_packet(buf,frame_l,packet,packet_l) == 0) //extraido e checksumado
			{
				int b = 0;
			}
		}
	}
	return 1;
}




